//ros库
#include <ros/ros.h>

//发布的位置消息体对应的头文件，该消息体的类型为geometry_msgs::PoseStamped
//用来进行发送目标位置
/*
ros官网上这样定义
# A Pose with reference coordinate frame and timestamp
Header header
Pose pose
实际上就是一个带有头消息和位姿的消息
*/
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/TwistWithCovariance.h>


//CommandBool服务的头文件，该服务的类型为mavros_msgs::CommandBool
//用来进行无人机解锁
/*
其结构如下（来源于ros wiki）
# Common type for switch commands
bool value
---
bool success
uint8 result
可以看到，发送的请求是一个bool类型的数据，为True则解锁，为False则上锁
返回的响应中success是一个bool类型的参数，表示上电/断电操作是否成功执行。
如果操作成功执行，success值为True，否则为False。
result是一个int32类型的参数，表示执行上电/断电操作的结果。
如果解锁/上锁操作成功执行，result值为0，
否则为其他值，表示执行解锁/上锁操作时发生了某种错误或异常。可以根据这个数值查看是哪种问题导致
*/
#include <mavros_msgs/CommandBool.h>

//SetMode服务的头文件，该服务的类型为mavros_msgs::SetMode
//用来设置无人机的飞行模式，切换offboard
/*
wiki上的消息定义如下
# set FCU mode
#
# Known custom modes listed here:
# http://wiki.ros.org/mavros/CustomModes

# basic modes from MAV_MODE
uint8 MAV_MODE_PREFLIGHT = 0
uint8 MAV_MODE_STABILIZE_DISARMED = 80
uint8 MAV_MODE_STABILIZE_ARMED = 208
uint8 MAV_MODE_MANUAL_DISARMED = 64
uint8 MAV_MODE_MANUAL_ARMED = 192
uint8 MAV_MODE_GUIDED_DISARMED = 88
uint8 MAV_MODE_GUIDED_ARMED = 216
uint8 MAV_MODE_AUTO_DISARMED = 92
uint8 MAV_MODE_AUTO_ARMED = 220
uint8 MAV_MODE_TEST_DISARMED = 66
uint8 MAV_MODE_TEST_ARMED = 194

uint8 base_mode # filled by MAV_MODE enum value or 0 if custom_mode != ''
string custom_mode # string mode representation or integer
---
bool success

String类型的变量custom_mode就是我们想切换的模式，有如下选择：
MANUAL，ACRO，ALTCTL，POSCTL，OFFBOARD，STABILIZED，RATTITUDE，AUTO.MISSION
AUTO.LOITER，AUTO.RTL，AUTO.LAND，AUTO.RTGS，AUTO.READY，AUTO.TAKEOFF
*/
#include <mavros_msgs/SetMode.h>
//订阅的消息体的头文件，该消息体的类型为mavros_msgs::State
//查看无人机的状态
/*
wiki上是这样的
std_msgs/Header header
bool connected
bool armed
bool guided
bool manual_input
string mode
uint8 system_status

解析如下：
header：消息头，包含时间戳和框架信息；
connected：表示是否连接到了 mavros 节点；
armed：表示无人机当前是否上锁；
guided：表示无人机当前是否处于 GUIDED 模式；
mode：表示当前无人机所处的模式，包括以下几种：
MANUAL，ACRO，ALTCTL，POSCTL，OFFBOARD，STABILIZED，RATTITUDE，AUTO.MISSION
AUTO.LOITER，AUTO.RTL，AUTO.LAND，AUTO.RTGS，AUTO.READY，AUTO.TAKEOFF
*/
#include <mavros_msgs/State.h> 

#include <mavros_msgs/PositionTarget.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>//可用来存储Double类型的数据
#include <sstream>


mavros_msgs::State current_state;//建立一个订阅消息体类型的变量，用于存储订阅的信息
geometry_msgs::PoseStamped local_pos;//用于存储订阅到的无人机的姿态信息（位置等）
geometry_msgs::PoseStamped last_local_pos;//上次订阅到的无人机的姿态信息（位置等）
//ConstPtr：常量智能指针，指向该消息的常量实例。这意味着你不能修改通过这个指针访问的对象。使用 ConstPtr 有助于确保消息在回调中保持不变，避免意外修改。
void state_cb(const mavros_msgs::State::ConstPtr& msg)//订阅时的回调函数，接受到该消息体的内容时执行里面的内容，内容是储存飞控当前的状态
{
    current_state = *msg;
}

void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    last_local_pos = local_pos; // 保存上次的姿态信息
    local_pos = *msg;           // 更新当前的姿态信息
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node"); //ros系统的初始化，argc和argv在后期节点传值会使用，最后一个参数offb_node为节点名称
    ros::NodeHandle nh;//实例化ROS句柄，这个ros::NodeHandle类封装了ROS中的一些常用功能
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);//这是一个订阅者对象，可以订阅无人机的状态信息（状态信息来源为MAVROS发布），用来储存无人机的状态，在回调函数中会不断更新这个状态变量
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 10, local_pos_callback);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);//这是一个发布者对象，用来在本地坐标系下发布目标点，后面会以20Hz频率发布目标点
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");//一个客户端，用来解锁无人机，这是因为无人机如果降落后一段时间没有收到信号输入，会自动上锁来保障安全
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");//一个客户端，用来切换飞行模式
    
    ros::Rate rate(20.0);

    while(ros::ok() && !current_state.connected)// 等待飞控和MAVROS建立连接，current_state是我订阅的MAVROS的状态，在收到心跳包之后连接成功跳出循环
    {
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;//实例化一个geometry_msgs::PoseStamped类型的对象，并对其赋值，最后将其发布出去
    pose.pose.position.x = 0;//尽管PX4在航空航天常用的NED坐标系下操控飞机，但MAVROS将自动将该坐标系切换至常规的ENU坐标系下
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //在进入Offboard模式之前，必须已经启动了local_pos_pub数据流，否则模式切换将被拒绝。
    //这里的100可以被设置为任意数
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    //定义一个变量来存储要设置的飞行模式
    /*
        可以设置的飞行模式为：
        MANUAL  手动
        ACRO    特技
        ALTCTL  高度
        POSCTL  定点
        OFFBOARD    外部模式
        STABILIZED  自稳
        RATTITUDE
        AUTO.LOITER     保持当前位置模式
        AUTO.RTL        自动返航模式
        AUTO.TAKEOFF    自动起飞模式
        AUTO.LAND       自动降落模式
        AUTO.MISSION    执行预设航点任务模式
        AUTO.RTGS       自动返回到航点（Return to GPS）
        AUTO.READY      准备模式。此模式表示无人机已准备好执行任务，但尚未启动。
    */
    ROS_INFO("will get the local position");

    //建立一个类型为CommandBool的服务端arm_cmd，并将其中的是否解锁设为"true"，作用便是用于后面的客户端与服务端之间的通信（服务）
    
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request_time = ros::Time::now();//更新判断模式时间
    ros::Time last_local_pos_time = ros::Time::now();//更新记录无人机姿态（如位置）的时间
    std_msgs::Float64 pos_errorx[3];//记录x轴误差,pos_error[0]表示这次误差，以此类推
    std_msgs::Float64 pos_errory[3];//记录y轴误差,pos_error[0]表示这次误差，以此类推
    std_msgs::Float64 pos_errorz[3];//记录z轴误差,pos_error[0]表示这次误差，以此类推
    for (int i = 0; i < 3; ++i) //初始化误差为10000米
    {
        pos_errorx[i].data = 10000;
        pos_errory[i].data = 10000;
        pos_errorz[i].data = 10000;
    }

    //大循环，只要节点还在ros::ok()的值就为正
    while(ros::ok())
    {
        if(ros::Time::now() - last_local_pos_time > ros::Duration(0.8))//每隔一段时间记录一下与目标位置的差值，若连续3次在误差内，则认为已经到达目标位置
        {
            ROS_WARN("local_pos.x,y,z:    %f,    %f,    %f",local_pos.pose.position.x,local_pos.pose.position.y,local_pos.pose.position.z);
            // //下面是判断和目标位置误差的，误差到一定范围内退出程序
            // pos_errorx[2].data = pos_errorx[1].data;//刷新误差
            // pos_errory[2].data = pos_errory[1].data;
            // pos_errorz[2].data = pos_errorz[1].data;

            // pos_errorx[1].data = pos_errorx[0].data;
            // pos_errory[1].data = pos_errory[0].data;
            // pos_errorz[1].data = pos_errorz[0].data;
            
            // pos_errorx[0].data = local_pos.pose.position.x - pose.pose.position.x;
            // pos_errory[0].data = local_pos.pose.position.y - pose.pose.position.y;
            // pos_errorz[0].data = local_pos.pose.position.z - pose.pose.position.z;
            // ROS_INFO("pos_errorx[0].data:%f,pos_errory[0].data:%f,pos_errorz[0].data:%f",pos_errorx[0].data,pos_errory[0].data,pos_errorz[0].data);
            // bool all_in_range = true;
            // for (int i = 0; i < 3; ++i) {
            //     if (!(pos_errorx[i].data > -0.6 && pos_errorx[i].data < 0.6) ||
            //         !(pos_errory[i].data > -0.6 && pos_errory[i].data < 0.6) ||
            //         !(pos_errorz[i].data > -0.6 && pos_errorz[i].data < 0.6)) {
            //         all_in_range = false;
            //         break; // 只要有一个不满足条件，就可以退出循环
            //     }
            // }
            // if (all_in_range) {
            //     // 所有数据都在 -0.6 到 0.6 之间，执行相应的逻辑
            //     ROS_INFO("the position is ok");
            //     // 在程序结束前切换到保持模式
            //     /*
            //     AUTO.LOITER     保持当前位置模式
            //     AUTO.RTL        自动返航模式
            //     AUTO.TAKEOFF    自动起飞模式
            //     AUTO.LAND       自动降落模式
            //     AUTO.MISSION    执行预设航点任务模式
            //     AUTO.RTGS       自动返回到航点（Return to GPS）
            //     AUTO.READY      准备模式。此模式表示无人机已准备好执行任务，但尚未启动。
            //     */
            //     mavros_msgs::SetMode hold_mode;
            //     hold_mode.request.custom_mode = "AUTO.LOITER";  //AUTO.LOITER:保持当前位置模式
            //     if (set_mode_client.call(hold_mode) && hold_mode.response.mode_sent) {
            //         ROS_INFO("Switched to LOITER mode. The drone will maintain its position.");
            //     }
            //     break;
            // }
            
            last_local_pos_time = ros::Time::now();//更新记录无人机姿态（如位置）的时间
        }
        ros::spinOnce();//当spinOnce函数被调用时，会调用回调函数队列中第一个回调函数，这里回调函数是state_cb函数
        rate.sleep();//根据前面ros::Rate rate(20.0);制定的发送频率自动休眠 休眠时间 = 1/频率
    }
    return 0;
}





